#include "SegmentsGraph.h"
#include "PointCloudSegmentation.h"
#include <Maths\Random.h>
#include <queue>

#include <boost\interprocess\file_mapping.hpp>
#include <boost\interprocess\mapped_region.hpp>
#include <boost\filesystem\operations.hpp>
#include <boost\filesystem\path.hpp>
#include <boost\filesystem.hpp>

#include <boost\lexical_cast.hpp>

using namespace Illumina::Core;

//Create a new SegmentsGraph and associate it with a specific PointCloudSegmentation instance
SegmentsGraph::SegmentsGraph(PointCloudSegmentation * p_pcs)
{
	m_pcs = p_pcs;
	m_disjointSegmentsGraphs = new std::map<int,SurfaceSegment*>();		
	//edgeID = new ::UniqueID();
	//stateID = new ::UniqueID();
	segmentID = new ::UniqueID();
	m_pointsPerSegment = new std::map<int, int>();
	m_surfaceSegments = new std::map<int, SurfaceSegment*>();
	m_prunedSurfaceSegments = new std::map<int, SurfaceSegment*>();
	m_edgeSegments = new std::map<int, EdgeSegment*>();	
	m_planePrimitivesOBB = new std::map<int, OBB*>();
	m_surfacePromotions = new std::map<int, int>();
}

	//Create an instance of a new Disjoint Graph covering a disjoint part of the point cloud
	//The state returned should be used to store the first segment processed.
SegmentsGraph::SurfaceSegment * SegmentsGraph::CreateAddDisjointGraph(int p_segmentId)
{
	//Create New State
	SurfaceSegment * newState = new SurfaceSegment();
	newState->id = p_segmentId;
	newState->label = "Initial";
	newState->connections = new std::map<int, EdgeSegment*>();
	(*m_disjointSegmentsGraphs)[p_segmentId] = newState;
	(*m_surfaceSegments)[p_segmentId] = newState;	
	std::cout << "[SceneSegmentsGraph] Created New Initial State ID=" << p_segmentId << std::endl;
	return newState;
}	

//Increase edge strength - NOT IMPLEMENTED
void SegmentsGraph::SetEdgeStrength(int p_edge, int p_strength)
{
	m_edgeSegmentsIterator = m_edgeSegments->find(p_edge);
	for (std::map<int, std::pair<int, SurfaceSegment*>>::iterator conn_iter = m_edgeSegmentsIterator->second->connections->begin(); 
		conn_iter != m_edgeSegmentsIterator->second->connections->end(); conn_iter++)
	{
		conn_iter->second.first = p_strength;
	}	
}

//Check if node exists depending on node type (surface, edge)
bool SegmentsGraph::SegmentExists(int p_nodeId, int p_typeId)
{
	if (p_typeId == 1)
		return SurfaceSegmentExists(p_nodeId);
	else
		return EdgeSegmentExists(p_nodeId);
}

//Check if node exists irrispective on node type (surface, edge)
bool SegmentsGraph::SegmentExists(int p_nodeId)
{
	return (SurfaceSegmentExists(p_nodeId) | EdgeSegmentExists(p_nodeId));
}

//Check if a state exists
bool SegmentsGraph::SurfaceSegmentExists(int p_stateId)
{
	if (m_surfaceSegments->find(p_stateId) != m_surfaceSegments->end())
		return true;
	else
		return false;
}

//Check if an edge exists
bool SegmentsGraph::EdgeSegmentExists(int p_stateId)
{
	if (m_edgeSegments->find(p_stateId) != m_edgeSegments->end())
		return true;
	else
		return false;
}				

//Create a new state in the graph. Id=segmentid
void SegmentsGraph::CreateNewSurfaceSegment(int p_id)
{	
	SurfaceSegment * newState = new SurfaceSegment();		
	newState->id = p_id;				
	(*m_surfaceSegments)[p_id] = newState;	
}

//Create a new state in the graph with next unique id
int SegmentsGraph::CreateNewSurfaceSegment()
{
	SurfaceSegment * newState = new SurfaceSegment();		
	//newState->id = m_surfaceSegments->size() + m_edgeSegments->size();
	newState->id = segmentID->GetNext();
	(*m_surfaceSegments)[newState->id] = newState;
	return newState->id;
}

int SegmentsGraph::CreateNewSurfaceSegment(SegmentsGraph::Shape p_primitive)
{	
	//Create New State
	SurfaceSegment * newState = new SurfaceSegment();		
	//newState->id = m_surfaceSegments->size() + m_edgeSegments->size();				
	newState->id = segmentID->GetNext();
	(*m_surfaceSegments)[newState->id] = newState;
	newState->shape_primitive = p_primitive;
	return newState->id;
}

//Create and return a new state in the graph
int SegmentsGraph::CreateNewEdgeSegment(int p_id)
{
	//Create New Edge
	if (p_id == -1) p_id = segmentID->GetNext();
	EdgeSegment * newEdge = new EdgeSegment();		
	newEdge->id = p_id;
	(*m_edgeSegments)[p_id] = newEdge;
	return p_id;
	//std::cout << "Creating a new Edge ID=" << p_id << std::endl;
}

//Adds connections between p_surface and p_edge	
void SegmentsGraph::AddConnection(SurfaceSegment * p_surface, EdgeSegment * p_edge) 
{	
	//First check whether connection is already present. 
	//If it's not add one. If it is increase strength.
	if (p_surface->connections->find(p_edge->id) == p_surface->connections->end())
	{
		(*(p_surface->connections))[p_edge->id] = p_edge;
		
		(*(p_edge->connections))[p_surface->id] = std::pair<int, SurfaceSegment *>(1, p_surface);

		//std::cout << "Added New Connection " << p_edge->id << "-" << p_surface->id << std::endl;
	} 
	else 
	{ 
		p_edge->connections->find(p_surface->id)->second.first++;
	}		
}	

//NOTE HUGE ASSUMPTION THAT p_node1 is of different type than p_node2
//Adds connections between p_edge and p_state
void SegmentsGraph::AddConnection(int p_segment1, int p_segment2) 
{
	EdgeSegment * edge;
	SurfaceSegment * state;
	if (SegmentExists(p_segment1) && SegmentExists(p_segment2)) //Check that both nodes exist
	{
		if (SurfaceSegmentExists(p_segment1))
		{
			state = m_surfaceSegments->find(p_segment1)->second;
			edge = m_edgeSegments->find(p_segment2)->second;
		}
		else
		{
			state = m_surfaceSegments->find(p_segment2)->second;
			edge = m_edgeSegments->find(p_segment1)->second;
		}

		if (p_segment1 == p_segment2)
			std::cout << "[SEGMENTS GRAPH WARNING] Adding Conn between segments " << p_segment1 << " and " << p_segment2 << std::endl;

		AddConnection(state, edge);
	}
}	
	
void SegmentsGraph::IncrementPointCountInSegment(int p_segmentId) 
{
	m_pointsPerSegmentIterator = m_pointsPerSegment->find(p_segmentId);
	if (m_pointsPerSegmentIterator != m_pointsPerSegment->end())
	{
		m_pointsPerSegmentIterator->second++;
	}
	else
	{
		(*m_pointsPerSegment)[p_segmentId] = 1;
	}
}

void SegmentsGraph::AddPoints(int p_segmentId, std::vector<int> &p_points)
{
	if (m_surfaceSegments->find(p_segmentId) != m_surfaceSegments->end())
	{
		SurfaceSegment * activeSurfaceSeg = m_surfaceSegments->find(p_segmentId)->second;
		for (int i=0; i<p_points.size(); i++) activeSurfaceSeg->points->push_back(p_points[i]);
	}
}

void SegmentsGraph::RemovePoints(int p_segmentId, std::vector<int> &p_points)
{
	if (m_surfaceSegments->find(p_segmentId) != m_surfaceSegments->end())
	{
		SurfaceSegment * activeSeg = m_surfaceSegments->find(p_segmentId)->second;
		std::vector<int> tmp;
		int crt_point;
		for (auto seg_points : *(activeSeg->points))
		{
			bool remove = false;
			for (int i=0; i<p_points.size(); i++)
			{
				if (p_points[i] == seg_points)
				{
					remove = true;
					break;
				}
			}
			if (!remove) tmp.push_back(seg_points);
		}

		activeSeg->points->clear();
		for (auto keep : tmp) activeSeg->points->push_back(keep);
	}

	if (m_edgeSegments->find(p_segmentId) != m_edgeSegments->end())
	{
		EdgeSegment * activeSeg = m_edgeSegments->find(p_segmentId)->second;
		std::vector<int> tmp;
		int crt_point;
		for (auto seg_points : *(activeSeg->points))
		{
			bool remove = false;
			for (int i=0; i<p_points.size(); i++)
			{
				if (p_points[i] == seg_points)
				{
					remove = true;
					break;
				}
			}
			if (!remove) tmp.push_back(seg_points);
		}

		activeSeg->points->clear();
		for (auto keep : tmp) activeSeg->points->push_back(keep);
	}
}

//Merge SourceId into SinkId and delete sourceId from the list of surface segments
void SegmentsGraph::MergeSurfaceSegments(int _sourceId, int _sinkId)
{
	m_surfaceSegmentsIterator = m_surfaceSegments->find(_sourceId);
	if (m_surfaceSegmentsIterator == m_surfaceSegments->end()) return;
	SurfaceSegment * source = m_surfaceSegmentsIterator->second;

	m_surfaceSegmentsIterator = m_surfaceSegments->find(_sinkId);
	if (m_surfaceSegmentsIterator == m_surfaceSegments->end()) return;
	SurfaceSegment * sink = m_surfaceSegmentsIterator->second;	

	for (int i=0; i<source->points->size(); i++) sink->points->push_back(source->points->at(i));
	//std::cout << "!!!!!!!!!!Added" << source->points.size() << " to segment " << sink->id << std::endl;

	EdgeSegment * crtEdgeSegment;
	//Go through all edge segments and remove (if any) the reference to this surface segment
	for (m_edgeSegmentsIterator = m_edgeSegments->begin(); m_edgeSegmentsIterator != m_edgeSegments->end(); m_edgeSegmentsIterator++)
	{
		crtEdgeSegment = m_edgeSegmentsIterator->second;
		if (crtEdgeSegment->connections->find(_sourceId) == crtEdgeSegment->connections->end()) continue;
		//std::cout << "!!!!!!!!!Deleting Surface Segment " << _sourceId << " from Edge Segment " << crtEdgeSegment->id  << std::endl;		
		crtEdgeSegment->connections->erase(_sourceId);
	}

	//std::cout << "!!!!!!!!!Deleting Surface Segment " << _sourceId << std::endl;	
	//std::cout << "[FULL SURFACE MERGE -" <<sink->id << "-] Merging " << source->id << "[" << source->points.size() << "] New Size=" << sink->points.size() << std::endl;
	//Finally assign the segment ids of the newly merged points.
	for (int i=0; i<source->points->size(); i++) m_pcs->m_pointCloud->m_pointList[source->points->at(i)].m_segmentId = sink->id;	
	m_surfaceSegments->erase(_sourceId);
	//Check if this surface segments is present in the disjoint segments graph.
	if (m_disjointSegmentsGraphs->find(_sourceId) != m_disjointSegmentsGraphs->end())
	{
		//Check whether the state we're merging into is accessible
		bool connected = false;
		for (std::map<int, SegmentsGraph::SurfaceSegment *>::iterator disjoint_iterator = m_disjointSegmentsGraphs->begin();
			disjoint_iterator != m_disjointSegmentsGraphs->end(); disjoint_iterator++)
		{
			if (disjoint_iterator->first != _sourceId)
			{
				std::vector<int> connected_surfaces = BreadthFirstTraversal(disjoint_iterator->second);
				for (int i=0; i<connected_surfaces.size(); i++) 
				{
					if (connected_surfaces[i] == _sinkId) connected = true;
				}
			}
			if (connected) break;
		}

		if(!connected) (*m_disjointSegmentsGraphs)[_sinkId] = sink;
		m_disjointSegmentsGraphs->erase(_sourceId);
	}

	//and safely delete the source node ... NOTE that this irreversible merging is done only to the segments graph.
	delete source; 	
}

/* UNFININSHED AND NOT USED
int SegmentsGraph::MergeSurfaceSegmentsWithEdgeSegments(int p_min_number_of_points)
{
	std::vector<int> small_segments;
	for (std::map<int, SegmentsGraph::SurfaceSegment*>::iterator si = m_surfaceSegments->begin(); si!=m_surfaceSegments->end(); si++)
	{
		if (PointCountInSegment(si->first) < p_min_number_of_points) 
		{
			//Iterator over the points			
			//if (si->second->connections->size() == 0) { small_segments.push_back(si->first); continue; }			
			small_segments.push_back(si->first);
		}
	}

	for (int i=0; i<small_segments.size(); i++)
	{
		m_surfaceSegments->erase(small_segments[i]);
		for (int j=0; j<m_disjointSegmentsGraphs->size(); j++)
		{
			if (m_disjointSegmentsGraphs->at(j)->id == small_segments[i];
		}
	}
}*/

//Prune from m_surfacesegments map ... include in the m_prunedSurfaceSegments map
void SegmentsGraph::PruneWeakSurfaceStates(int p_min_number_of_points, std::vector<int> &p_pruned_points)
{	
	//Iterator over surface and edge states. Remove any which have less than p_min_number_of_points.
	std::vector<int> p_pruned_surface_segments;
	std::vector<int> surface_segments_to_prune;	
	int prune_count = 0;
	int point_count = 0;
	for (std::map<int, SegmentsGraph::SurfaceSegment*>::iterator si = m_surfaceSegments->begin(); si!=m_surfaceSegments->end(); si++)
	{		
		if (PointCountInSegment(si->first) < p_min_number_of_points) 
		{
			(*m_prunedSurfaceSegments)[si->first] = si->second;
			p_pruned_surface_segments.push_back(si->first);
			prune_count++;
			point_count+=si->second->points->size();
			//std::cout << "Pruning Surface Segment #" << si->first << " which has " << PointCountInSegment(si->first) << std::endl;
		}
	}
	
	//Now iterate over the edge segments and remove the connections to the pruned surfaces
	for (int i=0; i<p_pruned_surface_segments.size(); i++)
	{		
		for (std::map<int, SegmentsGraph::EdgeSegment*>::iterator ei = m_edgeSegments->begin(); ei!=m_edgeSegments->end(); ei++)
		{
			if (ei->second->connections->find(p_pruned_surface_segments[i]) != ei->second->connections->end()) ei->second->connections->erase(p_pruned_surface_segments[i]);
		}

		std::vector<int> * p = m_surfaceSegments->at(p_pruned_surface_segments[i])->points;
		for (int j=0; j<p->size(); j++)
		{
			p_pruned_points.push_back(p->at(j));
		}
	}

	//Move pruned from m_surfaceSegments to m_prunedSurfaceSegments
	for (std::map<int, SurfaceSegment*>::iterator pruned_iterator = m_prunedSurfaceSegments->begin(); pruned_iterator != m_prunedSurfaceSegments->end(); pruned_iterator++)
	{
		m_surfaceSegments->erase(pruned_iterator->second->id);		
	}

	std::cout << "[SegmentsGraph Surface Pruning] Removed " << prune_count << " surface segments, totalling " << point_count << " points and" << std::endl;
	std::cout << "[SegmentsGraph Surface Pruning] Kept " << m_surfaceSegments->size() << " surface segments with more than " << p_min_number_of_points << " points/seg." << std::endl;
}

void SegmentsGraph::PruneEdgeSegmentsWithSingleConnection()
{
	std::vector<int> pruned;
	std::map<int, EdgeSegment*>::iterator edge_i;
	for (std::map<int, SegmentsGraph::EdgeSegment*>::iterator ei = m_edgeSegments->begin(); ei!=m_edgeSegments->end(); ei++)
	{
		if (ei->second->connections->size() == 1)
		{
			SegmentsGraph::SurfaceSegment * ss = ei->second->connections->begin()->second.second;			
			edge_i = ss->connections->find(ei->first);
			if (edge_i != ss->connections->end())
				ss->connections->erase(ss->connections->find(ei->first));
			else std::cout << "[SegmentsGraph Edge Pruning] Edge#" << ei->first << " not found in Surf#" << ss->id << std::endl;
			pruned.push_back(ei->first);
		}
	}

	for (int i=0; i<pruned.size(); i++) m_edgeSegments->erase(pruned[i]);
	std::cout << "[SegmentsGraph Edge Pruning] Removed " << pruned.size() << " edge segments having ONE surface connection" << std::endl;
}

int SegmentsGraph::PointCountInSegment(int p_segmentId) 
{
	m_surfaceSegmentsIterator = m_surfaceSegments->find(p_segmentId);
	if (m_surfaceSegmentsIterator != m_surfaceSegments->end())
		return m_surfaceSegmentsIterator->second->points->size();
	else
	{
		m_pointsPerSegmentIterator = m_pointsPerSegment->find(p_segmentId);
		if (m_pointsPerSegmentIterator != m_pointsPerSegment->end())
			return m_pointsPerSegmentIterator->second;
		else
			return -1;
	}
}

int SegmentsGraph::SurfaceSegmentsCount()
{
	return m_surfaceSegments->size();
}

SegmentsGraph::SurfaceSegment * SegmentsGraph::SurfaceSegmentAtIndex(int p_index)
{
	if (m_surfaceSegments->size() == 0) return NULL;
	std::map<int, SegmentsGraph::SurfaceSegment *>::iterator ssi = m_surfaceSegments->begin();	
	for (int i=0; i<p_index; i++)
	{
		ssi++;
		if (ssi == m_surfaceSegments->end()) return NULL;
	}
	return ssi->second;
}

SegmentsGraph::SurfaceSegment * SegmentsGraph::SurfaceSegmentWithId(int p_id)
{
	if (SurfaceSegmentExists(p_id)) return m_surfaceSegments->find(p_id)->second;
	return NULL;
}

SegmentsGraph::EdgeSegment * SegmentsGraph::EdgeSegmentWithId(int p_id)
{
	if (EdgeSegmentExists(p_id)) return m_edgeSegments->find(p_id)->second;
	return NULL;
}

// Returns a list of indices in breadth first order
std::vector<int> SegmentsGraph::BreadthFirstTraversal(SurfaceSegment * p_initialState)
{
	std::vector<int> traversal;
	std::map<int, bool> visited = std::map<int, bool>();
	for (m_surfaceSegmentsIterator = m_surfaceSegments->begin(); m_surfaceSegmentsIterator != m_surfaceSegments->end(); m_surfaceSegmentsIterator++)
	{
		visited[m_surfaceSegmentsIterator->first] = false;
	}

	std::map<int, std::pair<int, SurfaceSegment*>>::iterator localSurfaceSegmentsIterator;

	std::queue<SurfaceSegment*> toVisit = std::queue<SurfaceSegment*>();
	toVisit.push(p_initialState);
	while (!toVisit.empty())
	{
		traversal.push_back(toVisit.front()->id);
		visited[toVisit.front()->id] = true;
		std::map<int, EdgeSegment*> * connections = toVisit.front()->connections;			
		for (m_edgeSegmentsIterator = connections->begin(); m_edgeSegmentsIterator != connections->end(); m_edgeSegmentsIterator++) 
		{
			for (localSurfaceSegmentsIterator = m_edgeSegmentsIterator->second->connections->begin();
				localSurfaceSegmentsIterator != m_edgeSegmentsIterator->second->connections->end(); localSurfaceSegmentsIterator++)
			{
				if (!visited[localSurfaceSegmentsIterator->second.second->id])
				{
					toVisit.push((localSurfaceSegmentsIterator->second).second);
					visited[localSurfaceSegmentsIterator->second.second->id] = true;
					//std::cout << "Pushing " << surfaceSegmentsIterator->first << std::endl;
				}
			}
		}
		toVisit.pop();
	}
	return traversal;
}

std::vector<int> SegmentsGraph::GetConnectedEdgeSegments(std::vector<int> p_segs)
{
	std::vector<int> edgeSegments;
	std::map<int, int> edgeSegmentsMap;
	SegmentsGraph::SurfaceSegment * surfaceSeg;
	int s_id;
	for (auto p_segs_id : p_segs)
	{
		s_id = p_segs_id;
		if (m_surfacePromotions->find(p_segs_id) != m_surfacePromotions->end()) s_id = m_surfacePromotions->at(p_segs_id);
		surfaceSeg = SurfaceSegmentWithId(s_id);
		for (std::map<int, EdgeSegment*>::iterator connected_edge_iterator = surfaceSeg->connections->begin(); connected_edge_iterator != surfaceSeg->connections->end(); connected_edge_iterator++)
		{
			if (edgeSegmentsMap.find(connected_edge_iterator->first) == edgeSegmentsMap.end()) edgeSegmentsMap[connected_edge_iterator->first] = 0;
			edgeSegmentsMap[connected_edge_iterator->first]++;			
		}
	}

	for (std::map<int,int>::iterator edge_iterator = edgeSegmentsMap.begin(); edge_iterator != edgeSegmentsMap.end(); edge_iterator++) edgeSegments.push_back(edge_iterator->first);
		
	return edgeSegments;
}

std::vector<int> SegmentsGraph::GetEdgeSegmentsWithinSphere(Vector3 p_centre, float p_radius)
{
	std::vector<int> edgeSegments;		
	int s_id;
	std::vector<CloudPoint> point_list = m_pcs->m_pointCloud->m_pointList;
	float p_radius_sq = Maths::Sqr(p_radius);
	
	for (auto edges : *m_edgeSegments)
	{
		for (auto point_idx : *(edges.second->points))
		{
			if (Vector3::DistanceSquared(point_list.at(point_idx).m_vertex.Position, p_centre) < p_radius_sq) 
			{
				edgeSegments.push_back(edges.first);
				break;
			}
		}		
	}

	return edgeSegments;
}

SegmentsGraph::OBB SegmentsGraph::ComputeOBB(int _segmentId, float _buffer)
{
	SegmentsGraph::OBB obb;
	obb.buffer = _buffer;
	float c00, c11, c22, c01, c02, c12;
	c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;						
	Eigen::Matrix3f covariance = Eigen::Matrix3f();				
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver;
	Vector3 median = Vector3(0);
	SurfaceSegment * ss = m_surfaceSegments->find(_segmentId)->second;
	int numOfPoints = ss->points->size();

	for (int j=0; j<numOfPoints; j++)
	{					
		median.X += m_pcs->m_pointCloud->m_pointList[ss->points->at(j)].m_vertex.Position[0];
		median.Y += m_pcs->m_pointCloud->m_pointList[ss->points->at(j)].m_vertex.Position[1];
		median.Z += m_pcs->m_pointCloud->m_pointList[ss->points->at(j)].m_vertex.Position[2];							
	}
	median /= numOfPoints;

	//Now calculate diagonal of covariance matrix
	for (int j=0; j<numOfPoints; j++) 
	{			
		c00 += (m_pcs->m_pointCloud->m_pointList[ss->points->at(j)].m_vertex.Position[0] - median.X) * (m_pcs->m_pointCloud->m_pointList[ss->points->at(j)].m_vertex.Position[0] - median.X);
		c11 += (m_pcs->m_pointCloud->m_pointList[ss->points->at(j)].m_vertex.Position[1] - median.Y) * (m_pcs->m_pointCloud->m_pointList[ss->points->at(j)].m_vertex.Position[1] - median.Y);
		c22 += (m_pcs->m_pointCloud->m_pointList[ss->points->at(j)].m_vertex.Position[2] - median.Z) * (m_pcs->m_pointCloud->m_pointList[ss->points->at(j)].m_vertex.Position[2] - median.Z);
		c01 += (m_pcs->m_pointCloud->m_pointList[ss->points->at(j)].m_vertex.Position[0] - median.X) * (m_pcs->m_pointCloud->m_pointList[ss->points->at(j)].m_vertex.Position[1] - median.Y);
		c02 += (m_pcs->m_pointCloud->m_pointList[ss->points->at(j)].m_vertex.Position[0] - median.X) * (m_pcs->m_pointCloud->m_pointList[ss->points->at(j)].m_vertex.Position[2] - median.Z);
		c12 += (m_pcs->m_pointCloud->m_pointList[ss->points->at(j)].m_vertex.Position[1] - median.Y) * (m_pcs->m_pointCloud->m_pointList[ss->points->at(j)].m_vertex.Position[2] - median.Z);		
	}				

	covariance << c00 / numOfPoints, c01 / numOfPoints, c02 / numOfPoints, 
				c01 / numOfPoints, c11 / numOfPoints, c12 / numOfPoints, 
				c02 / numOfPoints, c12 / numOfPoints, c22 / numOfPoints;
				
	eigensolver.compute(covariance);			
	//std::cout << "[Eiegenvectors] " << eigensolver.eigenvectors().col(0) << " " << eigensolver.eigenvectors().col(1) << " " << eigensolver.eigenvectors().col(2) << " " << std::endl; 

	obb.obb_0 = Vector3(eigensolver.eigenvectors().col(0)[0], eigensolver.eigenvectors().col(0)[1], eigensolver.eigenvectors().col(0)[2]);				
	obb.obb_1 = Vector3(eigensolver.eigenvectors().col(1)[0], eigensolver.eigenvectors().col(1)[1], eigensolver.eigenvectors().col(1)[2]);
	obb.obb_2 = Vector3(eigensolver.eigenvectors().col(2)[0], eigensolver.eigenvectors().col(2)[1], eigensolver.eigenvectors().col(2)[2]);

	//Now move all the points in the coordinate space of the OBB
	std::vector<Vector3> OBBpoints;
	for (int i=0; i<numOfPoints; i++) OBBpoints.push_back(Vector3(Vector3::Dot(m_pcs->m_pointCloud->m_pointList[ss->points->at(i)].m_vertex.Position, obb.obb_0), 
																	Vector3::Dot(m_pcs->m_pointCloud->m_pointList[ss->points->at(i)].m_vertex.Position, obb.obb_1), 
																	Vector3::Dot(m_pcs->m_pointCloud->m_pointList[ss->points->at(i)].m_vertex.Position, obb.obb_2)));

	float obb_0_d_min = 1000000.0f; float obb_0_d_max = -1000000.0f;
	float obb_1_d_min = 1000000.0f; float obb_1_d_max = -1000000.0f;
	float obb_2_d_min = 1000000.0f; float obb_2_d_max = -1000000.0f;

	for (int i=0; i<OBBpoints.size(); i++) 
	{
		if (OBBpoints[i].X > obb_0_d_max) obb_0_d_max = OBBpoints[i].X;
		if (OBBpoints[i].Y > obb_1_d_max) obb_1_d_max = OBBpoints[i].Y;
		if (OBBpoints[i].Z > obb_2_d_max) obb_2_d_max = OBBpoints[i].Z;

		if (OBBpoints[i].X < obb_0_d_min) obb_0_d_min = OBBpoints[i].X;
		if (OBBpoints[i].Y < obb_1_d_min) obb_1_d_min = OBBpoints[i].Y;
		if (OBBpoints[i].Z < obb_2_d_min) obb_2_d_min = OBBpoints[i].Z;
	}

	obb.p_0_min = Plane(obb.obb_0, obb_0_d_min - _buffer);
	obb.p_0_max = Plane(obb.obb_0, obb_0_d_max + _buffer);
	obb.p_1_min = Plane(obb.obb_1, obb_1_d_min - _buffer);
	obb.p_1_max = Plane(obb.obb_1, obb_1_d_max + _buffer);
	obb.p_2_min = Plane(obb.obb_2, obb_2_d_min - _buffer);
	obb.p_2_max = Plane(obb.obb_2, obb_2_d_max + _buffer);
	
	return obb;
}

//Compute OBB for points in segments
SegmentsGraph::OBB SegmentsGraph::ComputeOBB(std::vector<int> _segmentIds, float _buffer)
{
	SegmentsGraph::OBB obb;
	obb.buffer = _buffer;
	float c00, c11, c22, c01, c02, c12;
	c00 = c11 = c22 = c01 = c02 = c12 = 0.0f;						
	Eigen::Matrix3f covariance = Eigen::Matrix3f();				
	Eigen::SelfAdjointEigenSolver<Eigen::Matrix3f> eigensolver;
	Vector3 median = Vector3(0);
	SurfaceSegment * ss;	

	std::vector<int> all_points;
	for (int i = 0; i<_segmentIds.size(); i++) 
	{
		ss = m_surfaceSegments->find(_segmentIds[i])->second;
		int numOfPointsSeg = ss->points->size();
		for (int j=0; j<numOfPointsSeg; j++)
		{
			all_points.push_back(ss->points->at(j));
		}
	}

	int numOfPoints = all_points.size();
	for (int j=0; j<numOfPoints; j++)
	{					
		median.X += m_pcs->m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[0];
		median.Y += m_pcs->m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[1];
		median.Z += m_pcs->m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[2];							
	}
	median /= numOfPoints;

	//Now calculate diagonal of covariance matrix
	for (int j=0; j<numOfPoints; j++) 
	{			
		c00 += (m_pcs->m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[0] - median.X) * (m_pcs->m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[0] - median.X);
		c11 += (m_pcs->m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[1] - median.Y) * (m_pcs->m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[1] - median.Y);
		c22 += (m_pcs->m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[2] - median.Z) * (m_pcs->m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[2] - median.Z);
		c01 += (m_pcs->m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[0] - median.X) * (m_pcs->m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[1] - median.Y);
		c02 += (m_pcs->m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[0] - median.X) * (m_pcs->m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[2] - median.Z);
		c12 += (m_pcs->m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[1] - median.Y) * (m_pcs->m_pointCloud->m_pointList[all_points[j]].m_vertex.Position[2] - median.Z);		
	}				

	covariance << c00 / numOfPoints, c01 / numOfPoints, c02 / numOfPoints, 
				c01 / numOfPoints, c11 / numOfPoints, c12 / numOfPoints, 
				c02 / numOfPoints, c12 / numOfPoints, c22 / numOfPoints;
				
	eigensolver.compute(covariance);			
	//std::cout << "[Eiegenvectors] " << eigensolver.eigenvectors().col(0) << " " << eigensolver.eigenvectors().col(1) << " " << eigensolver.eigenvectors().col(2) << " " << std::endl; 

	obb.obb_0 = Vector3(eigensolver.eigenvectors().col(0)[0], eigensolver.eigenvectors().col(0)[1], eigensolver.eigenvectors().col(0)[2]);				
	obb.obb_1 = Vector3(eigensolver.eigenvectors().col(1)[0], eigensolver.eigenvectors().col(1)[1], eigensolver.eigenvectors().col(1)[2]);
	obb.obb_2 = Vector3(eigensolver.eigenvectors().col(2)[0], eigensolver.eigenvectors().col(2)[1], eigensolver.eigenvectors().col(2)[2]);

	//Now move all the points in the coordinate space of the OBB
	std::vector<Vector3> OBBpoints;
	for (int i=0; i<numOfPoints; i++) OBBpoints.push_back(Vector3(Vector3::Dot(m_pcs->m_pointCloud->m_pointList[all_points[i]].m_vertex.Position, obb.obb_0), 
																	Vector3::Dot(m_pcs->m_pointCloud->m_pointList[all_points[i]].m_vertex.Position, obb.obb_1), 
																	Vector3::Dot(m_pcs->m_pointCloud->m_pointList[all_points[i]].m_vertex.Position, obb.obb_2)));

	float obb_0_d_min = 1000000.0f; float obb_0_d_max = -1000000.0f;
	float obb_1_d_min = 1000000.0f; float obb_1_d_max = -1000000.0f;
	float obb_2_d_min = 1000000.0f; float obb_2_d_max = -1000000.0f;

	for (int i=0; i<OBBpoints.size(); i++) 
	{
		if (OBBpoints[i].X > obb_0_d_max) obb_0_d_max = OBBpoints[i].X;
		if (OBBpoints[i].Y > obb_1_d_max) obb_1_d_max = OBBpoints[i].Y;
		if (OBBpoints[i].Z > obb_2_d_max) obb_2_d_max = OBBpoints[i].Z;

		if (OBBpoints[i].X < obb_0_d_min) obb_0_d_min = OBBpoints[i].X;
		if (OBBpoints[i].Y < obb_1_d_min) obb_1_d_min = OBBpoints[i].Y;
		if (OBBpoints[i].Z < obb_2_d_min) obb_2_d_min = OBBpoints[i].Z;
	}

	obb.p_0_min = Plane(obb.obb_0, obb_0_d_min - _buffer);
	obb.p_0_max = Plane(obb.obb_0, obb_0_d_max + _buffer);
	obb.p_1_min = Plane(obb.obb_1, obb_1_d_min - _buffer);
	obb.p_1_max = Plane(obb.obb_1, obb_1_d_max + _buffer);
	obb.p_2_min = Plane(obb.obb_2, obb_2_d_min - _buffer);
	obb.p_2_max = Plane(obb.obb_2, obb_2_d_max + _buffer);
	
	return obb;
}

//Determines whether the points in _segmentId_ are contained in the OBB of _segmentId_container
bool SegmentsGraph::InOBB(SurfaceSegment * _segment_container, SurfaceSegment * _segment_tocheck)
{
	SegmentsGraph::OBB crtOBB = _segment_container->obb;
	bool inOBB = true;
	Vector3 test_point_pos;
	for (int i=0; i<_segment_tocheck->points->size(); i++)
	{
		inOBB = false;
		test_point_pos = m_pcs->m_pointCloud->m_pointList[_segment_tocheck->points->at(i)].m_vertex.Position;
		if (crtOBB.p_0_min.GetSide(test_point_pos) == Plane::Side_Positive)
			if (crtOBB.p_0_max.GetSide(test_point_pos) == Plane::Side_Negative)
				if (crtOBB.p_1_min.GetSide(test_point_pos) == Plane::Side_Positive)
					if (crtOBB.p_1_max.GetSide(test_point_pos) == Plane::Side_Negative)
						if (crtOBB.p_2_min.GetSide(test_point_pos) == Plane::Side_Positive)
							if (crtOBB.p_2_max.GetSide(test_point_pos) == Plane::Side_Negative)
								inOBB = true;
		if (!inOBB) break;
	}
	return inOBB;
}

bool SegmentsGraph::InOBB(SurfaceSegment* _segment_container, Vector3 _position)
{
	SegmentsGraph::OBB crtOBB = _segment_container->obb;
	bool inOBB = true;
	Vector3 test_point_pos(_position.X, _position.Y, _position.Z);	
	inOBB = false;	
	if (crtOBB.p_0_min.GetSide(test_point_pos) == Plane::Side_Positive)
		if (crtOBB.p_0_max.GetSide(test_point_pos) == Plane::Side_Negative)
			if (crtOBB.p_1_min.GetSide(test_point_pos) == Plane::Side_Positive)
				if (crtOBB.p_1_max.GetSide(test_point_pos) == Plane::Side_Negative)
					if (crtOBB.p_2_min.GetSide(test_point_pos) == Plane::Side_Positive)
						if (crtOBB.p_2_max.GetSide(test_point_pos) == Plane::Side_Negative)
							inOBB = true;
	return inOBB;
}

bool SegmentsGraph::InOBB(SegmentsGraph::OBB _segment_obb, Vector3 _position)
{
	SegmentsGraph::OBB crtOBB = _segment_obb;
	bool inOBB = true;
	Vector3 test_point_pos(_position.X, _position.Y, _position.Z);	
	inOBB = false;	
	if (crtOBB.p_0_min.GetSide(test_point_pos) == Plane::Side_Positive)
		if (crtOBB.p_0_max.GetSide(test_point_pos) == Plane::Side_Negative)
			if (crtOBB.p_1_min.GetSide(test_point_pos) == Plane::Side_Positive)
				if (crtOBB.p_1_max.GetSide(test_point_pos) == Plane::Side_Negative)
					if (crtOBB.p_2_min.GetSide(test_point_pos) == Plane::Side_Positive)
						if (crtOBB.p_2_max.GetSide(test_point_pos) == Plane::Side_Negative)
							inOBB = true;
	return inOBB;
}

float SegmentsGraph::InOBBPercentage(SegmentsGraph::OBB p_obb, SegmentsGraph::SurfaceSegment * p_segment)
{
	int in_obb_count = 0;
	Vector3 test_point_pos;
	bool inOBB = false;
	for (int i=0; i<p_segment->points->size(); i++)
	{		
		inOBB = false;
		test_point_pos = m_pcs->m_pointCloud->m_pointList[p_segment->points->at(i)].m_vertex.Position;
		if (p_obb.p_0_min.GetSide(test_point_pos) == Plane::Side_Positive)
			if (p_obb.p_0_max.GetSide(test_point_pos) == Plane::Side_Negative)
				if (p_obb.p_1_min.GetSide(test_point_pos) == Plane::Side_Positive)
					if (p_obb.p_1_max.GetSide(test_point_pos) == Plane::Side_Negative)
						if (p_obb.p_2_min.GetSide(test_point_pos) == Plane::Side_Positive)
							if (p_obb.p_2_max.GetSide(test_point_pos) == Plane::Side_Negative)
								inOBB = true;
		if (inOBB) in_obb_count++;
	}
	//std::cout << "In OBB Count :: " << in_obb_count << std::endl;
	return (float)in_obb_count / (float)p_segment->points->size();
}

float SegmentsGraph::InOBBPercentage(SegmentsGraph::OBB p_obb, SegmentsGraph::EdgeSegment * p_segment)
{
	int in_obb_count = 0;
	Vector3 test_point_pos;
	bool inOBB = false;
	for (int i=0; i<p_segment->points->size(); i++)
	{		
		inOBB = false;
		test_point_pos = m_pcs->m_pointCloud->m_pointList[p_segment->points->at(i)].m_vertex.Position;
		if (p_obb.p_0_min.GetSide(test_point_pos) == Plane::Side_Positive)
			if (p_obb.p_0_max.GetSide(test_point_pos) == Plane::Side_Negative)
				if (p_obb.p_1_min.GetSide(test_point_pos) == Plane::Side_Positive)
					if (p_obb.p_1_max.GetSide(test_point_pos) == Plane::Side_Negative)
						if (p_obb.p_2_min.GetSide(test_point_pos) == Plane::Side_Positive)
							if (p_obb.p_2_max.GetSide(test_point_pos) == Plane::Side_Negative)
								inOBB = true;
		if (inOBB) in_obb_count++;
	}
	//std::cout << "In OBB Count :: " << in_obb_count << std::endl;
	return (float)in_obb_count / (float)p_segment->points->size();
}

//First checks OBB corners.
float SegmentsGraph::InOBBPercentageQuick(SegmentsGraph::OBB p_obb, SegmentsGraph::SurfaceSegment * p_segment)
{
	int in_obb_count = 0;
	Vector3 test_point_pos;
	bool inOBB = false;

	std::vector<Vector3> corners = p_segment->obb.GetOBBCorners();

	bool check = false;
	for (auto c : corners)
		if (this->InOBB(p_obb, c)) check=true;

	if (check)
	{
		for (int i=0; i<p_segment->points->size(); i++)
		{		
			inOBB = false;
			test_point_pos = m_pcs->m_pointCloud->m_pointList[p_segment->points->at(i)].m_vertex.Position;
			if (p_obb.p_0_min.GetSide(test_point_pos) == Plane::Side_Positive)
				if (p_obb.p_0_max.GetSide(test_point_pos) == Plane::Side_Negative)
					if (p_obb.p_1_min.GetSide(test_point_pos) == Plane::Side_Positive)
						if (p_obb.p_1_max.GetSide(test_point_pos) == Plane::Side_Negative)
							if (p_obb.p_2_min.GetSide(test_point_pos) == Plane::Side_Positive)
								if (p_obb.p_2_max.GetSide(test_point_pos) == Plane::Side_Negative)
									inOBB = true;
			if (inOBB) in_obb_count++;
		}
	
		//std::cout << "In OBB Count :: " << in_obb_count << std::endl;
		return (float)in_obb_count / (float)p_segment->points->size();
	}
	else return 0;

}

float SegmentsGraph::InOBBPercentage(SegmentsGraph::OBB p_obb, std::vector<SegmentsGraph::SurfaceSegment *> p_segments)
{
	int in_obb_count = 0;
	int total_count = 0;
	Vector3 test_point_pos;
	bool inOBB = false;
	SegmentsGraph::SurfaceSegment * p_segment;
	for (auto segments_it = p_segments.begin(); segments_it != p_segments.end(); segments_it++)
	{
		p_segment = *segments_it;
		for (int i=0; i<p_segment->points->size(); i++)
		{		
			inOBB = false;
			test_point_pos = m_pcs->m_pointCloud->m_pointList[p_segment->points->at(i)].m_vertex.Position;
			if (p_obb.p_0_min.GetSide(test_point_pos) == Plane::Side_Positive)
				if (p_obb.p_0_max.GetSide(test_point_pos) == Plane::Side_Negative)
					if (p_obb.p_1_min.GetSide(test_point_pos) == Plane::Side_Positive)
						if (p_obb.p_1_max.GetSide(test_point_pos) == Plane::Side_Negative)
							if (p_obb.p_2_min.GetSide(test_point_pos) == Plane::Side_Positive)
								if (p_obb.p_2_max.GetSide(test_point_pos) == Plane::Side_Negative)
									inOBB = true;
			if (inOBB) in_obb_count++;
		}
		total_count += p_segment->points->size();
	}
	//std::cout << "In OBB Count :: " << in_obb_count << std::endl;
	return (float)in_obb_count / (float)total_count;
}

//Create a GraphViz file describing the graph
void SegmentsGraph::WriteGraphToFile(std::string p_filepath)
{	
	std::ofstream graphfile;
	graphfile.open (p_filepath, std::ios::trunc);
	graphfile << "graph G\n\t{\n";
		
	//First write the state names with some statistics
	for (std::map<int, SegmentsGraph::SurfaceSegment*>::iterator si = m_surfaceSegments->begin(); si!=m_surfaceSegments->end(); si++)
	{
		graphfile << "\t\t" << si->first << " [Label=srf#" << PointCountInSegment(si->first) << "]\n";
	}

	for (std::map<int, SegmentsGraph::EdgeSegment*>::iterator ei = m_edgeSegments->begin(); ei!=m_edgeSegments->end(); ei++)
	{
		graphfile << "\t\t" << ei->first << " [Label=edg#" << PointCountInSegment(ei->first) << "]\n";
	}

	for (std::map<int, SegmentsGraph::EdgeSegment*>::iterator ei = m_edgeSegments->begin(); ei!=m_edgeSegments->end(); ei++)
	{
		graphfile << "subgraph cluster 0\n\t{\n";		
		for (std::map<int, std::pair<int, SegmentsGraph::SurfaceSegment*>>::iterator stateIterator = ei->second->connections->begin(); stateIterator != ei->second->connections->end(); stateIterator++)
		{			
			graphfile << "\t\t" << ei->first << "--" << stateIterator->second.second->id << " [Label=" << stateIterator->second.first << "]\n";
		}
	}
		
	graphfile << "}";
	graphfile.close();
}

//Write file with extension .bseg
void SegmentsGraph::WriteToBSEGFile(std::string _filepath)
{
	std::string pcsPath = _filepath+".bseg";		
	const char * filename = pcsPath.c_str();	
	
	int numberOfSurfaceSegments = m_surfaceSegments->size();
	int numberOfEdgeSegments = m_edgeSegments->size();

	//Determine size of file first
	int sizeSurfaceSegments = 0;
	for (m_surfaceSegmentsIterator = m_surfaceSegments->begin(); m_surfaceSegmentsIterator != m_surfaceSegments->end(); m_surfaceSegmentsIterator++) 
		sizeSurfaceSegments+=m_surfaceSegmentsIterator->second->sizeInBytes();

	int sizeEdgeSegments = 0;
	for (m_edgeSegmentsIterator = m_edgeSegments->begin(); m_edgeSegmentsIterator != m_edgeSegments->end(); m_edgeSegmentsIterator++) 
		sizeEdgeSegments+=m_edgeSegmentsIterator->second->sizeInBytes();

	int sizeDisjointGraphs = m_disjointSegmentsGraphs->size() * sizeof(int);

	int sizePointsPerSegmentMap = m_pointsPerSegment->size() * (2 * sizeof(int));

	OBB obb;

	int sizeOBBMap = (m_planePrimitivesOBB->size() * sizeof(int)) + (m_planePrimitivesOBB->size() * obb.sizeInBytes());

	size_t file_size = sizeof(int) + sizeSurfaceSegments + 
						sizeof(int) + sizeEdgeSegments + 
						sizeof(int) + sizeDisjointGraphs + 
						sizeof(int) + sizePointsPerSegmentMap +
						sizeof(int) + sizeOBBMap;
	
	std::filebuf fbuf;
	fbuf.open(filename, std::ios_base::in | std::ios_base::out | std::ios_base::trunc | std::ios_base::binary);	
	fbuf.pubseekoff( file_size - sizeof(char), std::ios_base::beg);
	fbuf.sputc(0);
	fbuf.close();

	size_t crtSize = 0;

	boost::interprocess::file_mapping m_file(filename, boost::interprocess::read_write);
	boost::interprocess::mapped_region region(m_file, boost::interprocess::read_write);						    

	int sizeint = sizeof(int);
	int sizefloat = sizeof(float);

	int * r_addr_i = (int*)region.get_address();			
	float * r_addr_f;
	size_t r_size = region.get_size();				
	std::cout << "[SegmentsGraph         ] Writing Bin File " << pcsPath << ". AllocSize= [" << r_size << "] " << std::endl;
	//std::cout << "[SegmentsGraph         ] Reserved SurfaceSegmentsSize :: " << sizeSurfaceSegments + sizeint << " #" << m_surfaceSegments->size() << std::endl;
	/*std::cout << "[SegmentsGraph         ] EdgeSegmentsSize    :: " << sizeEdgeSegments << " #" << m_edgeSegments->size() << std::endl;
	std::cout << "[SegmentsGraph         ] DisjointGraphsSize  :: " << sizeDisjointGraphs << " #" << m_disjointSegmentsGraphs->size() << std::endl;
	std::cout << "[SegmentsGraph         ] PointsPerSegMapSize :: " << sizePointsPerSegmentMap << std::endl;
	std::cout << "[SegmentsGraph         ] OBBMapSize          :: " << sizeOBBMap << std::endl;*/
	size_t idx = 0;
		
	Int64 file_start = (Int64)r_addr_i;
	Int64 surfaceSegmentsStart = (Int64)r_addr_i;
	int segsize = 0;
	*r_addr_i = numberOfSurfaceSegments; r_addr_i++;
	for (m_surfaceSegmentsIterator = m_surfaceSegments->begin(); m_surfaceSegmentsIterator != m_surfaceSegments->end(); m_surfaceSegmentsIterator++)
	{
		Int64 crtSurfaceSegmentsStart = (Int64)r_addr_i;
		int crtSegSize = 0;
		//std::cout << "Reserved Space for Segment = " << m_surfaceSegmentsIterator->second->sizeInBytes() << std::endl;		
		*r_addr_i = m_surfaceSegmentsIterator->second->id; r_addr_i++; crtSize+=sizeint; segsize+=sizeint; crtSegSize+=sizeint;
		//std::cout << "SURFACE_ID = " << m_surfaceSegmentsIterator->second->id << " #points=" << m_surfaceSegmentsIterator->second->points->size() << std::endl;

		//Write Points in Segment
		*r_addr_i = m_surfaceSegmentsIterator->second->points->size(); r_addr_i++; crtSize+=sizeint; segsize+=sizeint; crtSegSize+=sizeint;
		for (int i=0; i<m_surfaceSegmentsIterator->second->points->size(); i++)
		{
			*r_addr_i = m_surfaceSegmentsIterator->second->points->at(i); r_addr_i++;
		}

		//std::cout << "---" << m_surfaceSegmentsIterator->second->points->size() << " Points Written Mem Used :: " << (Int64)r_addr_i - crtSurfaceSegmentsStart << " crtSegSize :: " << crtSegSize << std::endl;

		//Write Connected Edge Segments
		crtSurfaceSegmentsStart = (Int64)r_addr_i;
		crtSegSize = 0;
		*r_addr_i = m_surfaceSegmentsIterator->second->connections->size(); r_addr_i++;
		for (std::map<int, EdgeSegment*>::iterator it = m_surfaceSegmentsIterator->second->connections->begin(); it != m_surfaceSegmentsIterator->second->connections->end(); it++)
		{
			*r_addr_i = it->first; r_addr_i++;
		}
		//std::cout << "---" << m_surfaceSegmentsIterator->second->connections->size() << " Conns Written Mem Used :: " << (Int64)r_addr_i - crtSurfaceSegmentsStart << " crtSegSize :: " << crtSegSize << std::endl;

		crtSurfaceSegmentsStart = (Int64)r_addr_i;
		crtSegSize = 0;
		*r_addr_i = m_surfaceSegmentsIterator->second->shape_primitive; r_addr_i++; crtSize+=sizeof(Shape); segsize+=sizeof(Shape); crtSegSize+=sizeof(Shape);

		if (m_surfaceSegmentsIterator->second->shape_primitive != Shape::compound)
		{
			*r_addr_i = 0; r_addr_i++;
		}
		else 
		{
			*r_addr_i = 1; r_addr_i++; crtSize+=sizeint; segsize+=sizeint; crtSegSize+=sizeint;
			m_surfaceSegmentsIterator->second->internalSegGraph->WriteToBSEGFile(_filepath+"_s"+boost::lexical_cast<std::string, int>(m_surfaceSegmentsIterator->first)+"_i");
			m_surfaceSegmentsIterator->second->internalStrGraph->WriteToBSTRFile(_filepath+"_s"+boost::lexical_cast<std::string, int>(m_surfaceSegmentsIterator->first)+"_i");
		}	

		*r_addr_i = m_surfaceSegmentsIterator->second->internalGraphSelectionIndex; r_addr_i++;
		*r_addr_i = m_surfaceSegmentsIterator->second->visibilityCount; r_addr_i++;
		*r_addr_i = m_surfaceSegmentsIterator->second->maxDecompositionCount; r_addr_i++;

		r_addr_f = (float*)r_addr_i;
		*r_addr_f = m_surfaceSegmentsIterator->second->coverage; r_addr_f++;
		r_addr_i = (int*)r_addr_f;

		//std::cout << "---Max Decomp Mem Used :: " << (Int64)r_addr_i - crtSurfaceSegmentsStart << " crtSegSize :: " << crtSegSize << std::endl;

		crtSurfaceSegmentsStart = (Int64)r_addr_i;
		crtSegSize = 0;

		r_addr_f = (float*)r_addr_i;
		*r_addr_f = m_surfaceSegmentsIterator->second->bounding_min.X; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->bounding_min.Y; r_addr_f++; 
		*r_addr_f = m_surfaceSegmentsIterator->second->bounding_min.Z; r_addr_f++; 

		*r_addr_f = m_surfaceSegmentsIterator->second->bounding_max.X; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->bounding_max.Y; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->bounding_max.Z; r_addr_f++;

		*r_addr_f = m_surfaceSegmentsIterator->second->orientation.X; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->orientation.Y; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->orientation.Z; r_addr_f++;
						
		r_addr_i = (int*)r_addr_f;
		//std::cout << "---Bounding/Orientation Done. Mem Used :: " << (Int64)r_addr_i - crtSurfaceSegmentsStart << ", crtSegSize :: " << crtSegSize << std::endl;

		crtSurfaceSegmentsStart = (Int64)r_addr_i;
		crtSegSize = 0;

		int obbsize = 0;
		//Finally Persist OBB
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.obb_0.X; r_addr_f++; 
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.obb_0.Y; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.obb_0.Z; r_addr_f++;

		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_0_min.Normal.X; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_0_min.Normal.Y; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_0_min.Normal.Z; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_0_min.Distance; r_addr_f++;
		 
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_0_max.Normal.X; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_0_max.Normal.Y; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_0_max.Normal.Z; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_0_max.Distance; r_addr_f++;

		*r_addr_f = m_surfaceSegmentsIterator->second->obb.obb_1.X; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.obb_1.Y; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.obb_1.Z; r_addr_f++;
		 
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_1_min.Normal.X; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_1_min.Normal.Y; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_1_min.Normal.Z; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_1_min.Distance; r_addr_f++;

		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_1_max.Normal.X; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_1_max.Normal.Y; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_1_max.Normal.Z; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_1_max.Distance; r_addr_f++;

		*r_addr_f = m_surfaceSegmentsIterator->second->obb.obb_2.X; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.obb_2.Y; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.obb_2.Z; r_addr_f++;

		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_2_min.Normal.X; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_2_min.Normal.Y; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_2_min.Normal.Z; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_2_min.Distance; r_addr_f++;

		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_2_max.Normal.X; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_2_max.Normal.Y; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_2_max.Normal.Z; r_addr_f++;
		*r_addr_f = m_surfaceSegmentsIterator->second->obb.p_2_max.Distance; r_addr_f++;

		*r_addr_f = m_surfaceSegmentsIterator->second->obb.buffer; r_addr_f++;
		
		r_addr_i = (int*)r_addr_f;
		//std::cout << "---OBB Done Mem Used :: " << (Int64)r_addr_i - crtSurfaceSegmentsStart << ", crtSegSize :: " << crtSegSize << std::endl;

	}

	//std::cout << "+++All Surfaces Segments Written. Size Alloc :: " << sizeof(int) + sizeSurfaceSegments << ", Used :: " << (Int64)r_addr_i - surfaceSegmentsStart << ", SegSize :: " << segsize << std::endl;
	
	//Now Persist Edge Segments
	r_addr_i = (int*)r_addr_f;
	int edgeSize = 0;
	Int64 edgeSegmentsStart = (Int64)r_addr_i;
	*r_addr_i = numberOfEdgeSegments; r_addr_i++;
	for (m_edgeSegmentsIterator = m_edgeSegments->begin(); m_edgeSegmentsIterator != m_edgeSegments->end(); m_edgeSegmentsIterator++) 
	{
		Int64 crtEdgeSegmentStart = (Int64)r_addr_i;
		int crtSegSize = 0;		
		*r_addr_i = m_edgeSegmentsIterator->second->id; r_addr_i++; 
		*r_addr_i = m_edgeSegmentsIterator->second->occlusion; r_addr_i++; 
		
		//Write Points
		*r_addr_i = m_edgeSegmentsIterator->second->points->size(); r_addr_i++;
		//std::cout << "EdgeID = " << m_edgeSegmentsIterator->second->id << " #points=" << m_edgeSegmentsIterator->second->points->size() << std::endl;
		for (int i=0; i<m_edgeSegmentsIterator->second->points->size(); i++) 
		{			
			*r_addr_i = m_edgeSegmentsIterator->second->points->at(i); r_addr_i++;
		}

		//Write Connections
		*r_addr_i = m_edgeSegmentsIterator->second->connections->size(); r_addr_i++;
		for (std::map<int, std::pair<int, SurfaceSegment*>>::iterator it = m_edgeSegmentsIterator->second->connections->begin(); 
			it != m_edgeSegmentsIterator->second->connections->end(); it++)
		{
			*r_addr_i = it->first; r_addr_i++;
			*r_addr_i = it->second.first; r_addr_i++;
		}
		//std::cout << "---Edge Written Size Alloc :: " << m_edgeSegmentsIterator->second->sizeInBytes() << ", Used :: " << (Int64)r_addr_i - crtEdgeSegmentStart << ", edgeSize :: " << crtSegSize << std::endl;
	}	

	//std::cout << "---All Edge Segments Written. Size Alloc :: " << sizeof(int) + sizeEdgeSegments << ", Used :: " << (Int64)r_addr_i - edgeSegmentsStart << ", edgeSize :: " << edgeSize << std::endl;
	
	//Now Persist Ids of initial nodes of Disjoint Graphs
	*r_addr_i = m_disjointSegmentsGraphs->size(); r_addr_i++;
	//std::cout << "*** Disjoint size :: " << m_disjointSegmentsGraphs->size();
	for (std::map<int, SurfaceSegment*>::iterator it = m_disjointSegmentsGraphs->begin(); it != m_disjointSegmentsGraphs->end(); it++)
	{		
		*r_addr_i = it->first; r_addr_i++;
		//std::cout << " " << it->first;
	}
	//std::cout << std::endl;

	//std::cout << "Crt Size - Disjoint Written :: " << crtSize << std::endl;

	//Now Persist Points / Segment Map
	*r_addr_i = m_pointsPerSegment->size(); r_addr_i++;
	for (std::map<int, int>::iterator it = m_pointsPerSegment->begin(); it != m_pointsPerSegment->end(); it++)
	{
		*r_addr_i = it->first; r_addr_i++;
		*r_addr_i = it->second; r_addr_i++;
	}

	//std::cout << "Crt Size - Points/Segment Written :: " << crtSize << std::endl;

	//Finally Persist OBBs
	*r_addr_i = m_planePrimitivesOBB->size(); r_addr_i++;
	for (std::map<int, OBB*>::iterator it = m_planePrimitivesOBB->begin(); it != m_planePrimitivesOBB->end(); it++)
	{				
		*r_addr_i = it->first; r_addr_i++;
	}

	r_addr_f = (float*) r_addr_i;
	for (std::map<int, OBB*>::iterator it = m_planePrimitivesOBB->begin(); it != m_planePrimitivesOBB->end(); it++)
	{				
		*r_addr_f = it->second->obb_0.X; r_addr_f++;
		*r_addr_f = it->second->obb_0.Y; r_addr_f++;
		*r_addr_f = it->second->obb_0.Z; r_addr_f++;

		*r_addr_f = it->second->p_0_min.Normal.X; r_addr_f++;
		*r_addr_f = it->second->p_0_min.Normal.Y; r_addr_f++; 
		*r_addr_f = it->second->p_0_min.Normal.Z; r_addr_f++;
		*r_addr_f = it->second->p_0_min.Distance; r_addr_f++;

		*r_addr_f = it->second->p_0_max.Normal.X; r_addr_f++;
		*r_addr_f = it->second->p_0_max.Normal.Y; r_addr_f++; 
		*r_addr_f = it->second->p_0_max.Normal.Z; r_addr_f++;
		*r_addr_f = it->second->p_0_max.Distance; r_addr_f++;

		*r_addr_f = it->second->obb_1.X; r_addr_f++;
		*r_addr_f = it->second->obb_1.Y; r_addr_f++;
		*r_addr_f = it->second->obb_1.Z; r_addr_f++;

		*r_addr_f = it->second->p_1_min.Normal.X; r_addr_f++;
		*r_addr_f = it->second->p_1_min.Normal.Y; r_addr_f++;
		*r_addr_f = it->second->p_1_min.Normal.Z; r_addr_f++;
		*r_addr_f = it->second->p_1_min.Distance; r_addr_f++;

		*r_addr_f = it->second->p_1_max.Normal.X; r_addr_f++;
		*r_addr_f = it->second->p_1_max.Normal.Y; r_addr_f++;
		*r_addr_f = it->second->p_1_max.Normal.Z; r_addr_f++;
		*r_addr_f = it->second->p_1_max.Distance; r_addr_f++;

		*r_addr_f = it->second->obb_2.X; r_addr_f++;
		*r_addr_f = it->second->obb_2.Y; r_addr_f++;
		*r_addr_f = it->second->obb_2.Z; r_addr_f++;

		*r_addr_f = it->second->p_2_min.Normal.X; r_addr_f++;
		*r_addr_f = it->second->p_2_min.Normal.Y; r_addr_f++;
		*r_addr_f = it->second->p_2_min.Normal.Z; r_addr_f++;
		*r_addr_f = it->second->p_2_min.Distance; r_addr_f++;

		*r_addr_f = it->second->p_2_max.Normal.X; r_addr_f++;
		*r_addr_f = it->second->p_2_max.Normal.Y; r_addr_f++;
		*r_addr_f = it->second->p_2_max.Normal.Z; r_addr_f++;
		*r_addr_f = it->second->p_2_max.Distance; r_addr_f++;

		*r_addr_f = it->second->buffer; r_addr_f++;
	}	
	
	r_addr_i = (int*) r_addr_f;
	
	region.flush();
	
	std::cout << "[SegmentsGraph         ] Writing Bin File " << pcsPath << ". WriteSize= [" << (Int64)r_addr_i - file_start << "] " << std::endl;	
}

//Read files of type .bseg
bool SegmentsGraph::ReadFromBSEGFile(std::string _filepath)
{
	std::string pcsPath = _filepath+".bseg";		
	const char * filename = pcsPath.c_str();	
		
	boost::filesystem::path p (pcsPath, boost::filesystem::native);
	if ( !boost::filesystem::exists( p ) )
	{
		std::cerr << "Error : Couldn't open file '" << pcsPath << "'" << std::endl;
		return false;
	}
		
	//we assume all points/file fit in main memory
	boost::interprocess::file_mapping m_file;		
	boost::interprocess::mapped_region *m_region;
	
	m_file = boost::interprocess::file_mapping(filename, boost::interprocess::read_only);
	m_region = new boost::interprocess::mapped_region(m_file, boost::interprocess::read_only);					
	float * r_addr_f = (float*)m_region->get_address();
	int * r_addr_i = (int*)m_region->get_address();
	Int64 file_start = (Int64)r_addr_i;
	size_t r_size = m_region->get_size();					
	int numberOfSurfaceSegments = (int)*r_addr_i; r_addr_i++;

	std::cout << "[SegmentsGraph         ] Reading Bin File " << pcsPath << ". FileSize= [" << r_size << "]" << std::endl;	

	std::map<int, std::vector<int>> surfaceEdgeConnections;

	//std::cout << "[SegmentsGraph         ] SurfaceSegments Count :: " << numberOfSurfaceSegments << std::endl;

	for (int i=0; i<numberOfSurfaceSegments; i++)
	{		
		int id = (int)*r_addr_i; r_addr_i++;
		CreateNewSurfaceSegment(id);

		//Write Points to Segment
		int pointsCount = (int)*r_addr_i; r_addr_i++;
		SurfaceSegment * crtSurfaceSegment = m_surfaceSegments->at(id);
		for (int j=0; j<pointsCount; j++)
		{
			crtSurfaceSegment->points->push_back((int)*r_addr_i); r_addr_i++;			
		}

		//Write Connected Edge Segments
		int connectionsCount = (int)*r_addr_i; r_addr_i++;		
		for (int j=0; j<connectionsCount; j++)
		{
			surfaceEdgeConnections[id].push_back((int)*r_addr_i); r_addr_i++;
		}

		crtSurfaceSegment->shape_primitive = (Shape)*r_addr_i; r_addr_i++;		
		int isCompound = (int)*r_addr_i; r_addr_i++;

		if (isCompound == 1)
		{
			//Load internal segment and structure graph of this segment.
			crtSurfaceSegment->internalSegGraph = new SegmentsGraph(m_pcs);
			crtSurfaceSegment->internalStrGraph = new StructureGraph(crtSurfaceSegment->internalSegGraph);

			crtSurfaceSegment->internalSegGraph->ReadFromBSEGFile(_filepath+"_s"+boost::lexical_cast<std::string, int>(id)+"_i");						
			crtSurfaceSegment->internalStrGraph->ReadFromBSTRFile(_filepath+"_s"+boost::lexical_cast<std::string, int>(id)+"_i");			
		}		

		crtSurfaceSegment->internalGraphSelectionIndex = (int)*r_addr_i; r_addr_i++;
		crtSurfaceSegment->visibilityCount = (int)*r_addr_i; r_addr_i++;
		crtSurfaceSegment->maxDecompositionCount = (int)*r_addr_i; r_addr_i++;
		r_addr_f = (float*)r_addr_i;
		crtSurfaceSegment->coverage = (float)*r_addr_f; r_addr_f++;
		r_addr_i = (int*)r_addr_f;

		r_addr_f = (float*)r_addr_i;

		crtSurfaceSegment->bounding_min.X = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->bounding_min.Y = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->bounding_min.Z = (float)*r_addr_f; r_addr_f++;
		
		crtSurfaceSegment->bounding_max.X = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->bounding_max.Y = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->bounding_max.Z = (float)*r_addr_f; r_addr_f++;

		crtSurfaceSegment->orientation.X = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->orientation.Y = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->orientation.Z = (float)*r_addr_f; r_addr_f++;		
																				
		//Read OBB
		crtSurfaceSegment->obb.obb_0.X = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.obb_0.Y = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.obb_0.Z = (float)*r_addr_f; r_addr_f++;

		crtSurfaceSegment->obb.p_0_min.Normal.X = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_0_min.Normal.Y = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_0_min.Normal.Z = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_0_min.Distance = (float)*r_addr_f; r_addr_f++;		

		crtSurfaceSegment->obb.p_0_max.Normal.X = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_0_max.Normal.Y = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_0_max.Normal.Z = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_0_max.Distance = (float)*r_addr_f; r_addr_f++;

		crtSurfaceSegment->obb.obb_1.X = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.obb_1.Y = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.obb_1.Z = (float)*r_addr_f; r_addr_f++;

		crtSurfaceSegment->obb.p_1_min.Normal.X = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_1_min.Normal.Y = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_1_min.Normal.Z = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_1_min.Distance = (float)*r_addr_f; r_addr_f++;		

		crtSurfaceSegment->obb.p_1_max.Normal.X = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_1_max.Normal.Y = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_1_max.Normal.Z = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_1_max.Distance = (float)*r_addr_f; r_addr_f++;

		crtSurfaceSegment->obb.obb_2.X = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.obb_2.Y = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.obb_2.Z = (float)*r_addr_f; r_addr_f++;

		crtSurfaceSegment->obb.p_2_min.Normal.X = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_2_min.Normal.Y = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_2_min.Normal.Z = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_2_min.Distance = (float)*r_addr_f; r_addr_f++;		

		crtSurfaceSegment->obb.p_2_max.Normal.X = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_2_max.Normal.Y = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_2_max.Normal.Z = (float)*r_addr_f; r_addr_f++;
		crtSurfaceSegment->obb.p_2_max.Distance = (float)*r_addr_f; r_addr_f++;

		crtSurfaceSegment->obb.buffer = (float)*r_addr_f; r_addr_f++;

		r_addr_i = (int*)r_addr_f;
	}	

	//Now Read Edge Segments
	r_addr_i = (int*)r_addr_f;
	int numberOfEdgeSegments = (int)*r_addr_i; r_addr_i++;

	//std::cout << "[SegmentsGraph         ] EdgeSegmentsSize    :: " << numberOfEdgeSegments << std::endl;
	
	for (int i=0;i<numberOfEdgeSegments; i++) 
	{
		int id = (int)*r_addr_i; r_addr_i++;
		CreateNewEdgeSegment(id);

		EdgeSegment * crtEdgeSegment = m_edgeSegments->at(id);
		crtEdgeSegment->occlusion = (int)*r_addr_i; r_addr_i++;
		
		//Write Points
		int pointCount = (int)*r_addr_i; r_addr_i++;
		//std::cout << "EdgeID = " << crtEdgeSegment->id << " #points=" << pointCount << std::endl;
		for (int j=0; j<pointCount; j++) 
		{
			crtEdgeSegment->points->push_back((int)*r_addr_i); r_addr_i++;
		}

		//Write Connections
		int numberOfConnections = (int)*r_addr_i; r_addr_i++;
		for (int j=0; j<numberOfConnections; j++)
		{
			int sId = (int)*r_addr_i; r_addr_i++;
			int str = (int)*r_addr_i; r_addr_i++;
			(*(crtEdgeSegment->connections))[sId] = std::pair<int, SurfaceSegment*>(str, m_surfaceSegments->at(sId));			
		}		
	}	


	//Now Read Ids of initial nodes of Disjoint Graphs
	int numberOfDisjointGraphs = (int)*r_addr_i; r_addr_i++;
	//std::cout << "Disjoint Size :: " << numberOfDisjointGraphs;
	for (int j=0; j<numberOfDisjointGraphs; j++)
	{
		int sId = (int)*r_addr_i; r_addr_i++;
		//std::cout << " " << sId;
		(*m_disjointSegmentsGraphs)[sId] = m_surfaceSegments->at(sId);
	}	

	//Now Persist Points / Segment Map
	int countPointsPerSegmentMap = (int)*r_addr_i; r_addr_i++;	
	for (int j=0; j<countPointsPerSegmentMap; j++)
	{
		int sId = (int)*r_addr_i; r_addr_i++;
		int pCount = (int)*r_addr_i; r_addr_i++;
		(*m_pointsPerSegment)[sId] = pCount;		
	}	

	//Finally Persist OBBs
	std::vector<int> obbs;
	int countOBBs = (int)*r_addr_i; r_addr_i++;
	for (int j=0; j<countOBBs; j++)
	{	
		int sId = (int)*r_addr_i; r_addr_i++;
		obbs.push_back(sId);
	}

	r_addr_f = (float*) r_addr_i;
	for (int j=0; j<countOBBs; j++)
	{				
		OBB *obb = new OBB();
		obb->obb_0.X = (float)*r_addr_f; r_addr_f++;
		obb->obb_0.Y = (float)*r_addr_f; r_addr_f++;
		obb->obb_0.Z = (float)*r_addr_f; r_addr_f++;

		obb->p_0_min.Normal.X = (float)*r_addr_f; r_addr_f++;
		obb->p_0_min.Normal.Y = (float)*r_addr_f; r_addr_f++;
		obb->p_0_min.Normal.Z = (float)*r_addr_f; r_addr_f++;
		obb->p_0_min.Distance = (float)*r_addr_f; r_addr_f++;

		obb->p_0_max.Normal.X = (float)*r_addr_f; r_addr_f++;
		obb->p_0_max.Normal.Y = (float)*r_addr_f; r_addr_f++;
		obb->p_0_max.Normal.Z = (float)*r_addr_f; r_addr_f++;
		obb->p_0_max.Distance = (float)*r_addr_f; r_addr_f++;		

		obb->obb_1.X = (float)*r_addr_f; r_addr_f++;
		obb->obb_1.Y = (float)*r_addr_f; r_addr_f++;
		obb->obb_1.Z = (float)*r_addr_f; r_addr_f++;

		obb->p_1_min.Normal.X = (float)*r_addr_f; r_addr_f++;
		obb->p_1_min.Normal.Y = (float)*r_addr_f; r_addr_f++;
		obb->p_1_min.Normal.Z = (float)*r_addr_f; r_addr_f++;
		obb->p_1_min.Distance = (float)*r_addr_f; r_addr_f++;

		obb->p_1_max.Normal.X = (float)*r_addr_f; r_addr_f++;
		obb->p_1_max.Normal.Y = (float)*r_addr_f; r_addr_f++;
		obb->p_1_max.Normal.Z = (float)*r_addr_f; r_addr_f++;
		obb->p_1_max.Distance = (float)*r_addr_f; r_addr_f++;

		obb->obb_2.X = (float)*r_addr_f; r_addr_f++;
		obb->obb_2.Y = (float)*r_addr_f; r_addr_f++;
		obb->obb_2.Z = (float)*r_addr_f; r_addr_f++;

		obb->p_2_min.Normal.X = (float)*r_addr_f; r_addr_f++;
		obb->p_2_min.Normal.Y = (float)*r_addr_f; r_addr_f++;
		obb->p_2_min.Normal.Z = (float)*r_addr_f; r_addr_f++;
		obb->p_2_min.Distance = (float)*r_addr_f; r_addr_f++;

		obb->p_2_max.Normal.X = (float)*r_addr_f; r_addr_f++;
		obb->p_2_max.Normal.Y = (float)*r_addr_f; r_addr_f++;
		obb->p_2_max.Normal.Z = (float)*r_addr_f; r_addr_f++;
		obb->p_2_max.Distance = (float)*r_addr_f; r_addr_f++;	

		obb->buffer = (float)*r_addr_f; r_addr_f++;	
		r_addr_i = (int*) r_addr_f;

		(*m_planePrimitivesOBB)[obbs[j]] = obb;
	}			

	//Finally write the surface to edge connections for all surface segments
	for (std::map<int, std::vector<int>>::iterator surfaceEdgeConnectionsIterator = surfaceEdgeConnections.begin(); 
		surfaceEdgeConnectionsIterator!=surfaceEdgeConnections.end(); surfaceEdgeConnectionsIterator++)
	{
		for (int i=0; i<surfaceEdgeConnectionsIterator->second.size(); i++)
		{
			if (m_edgeSegments->find(surfaceEdgeConnectionsIterator->second[i]) != m_edgeSegments->end())
			(*(m_surfaceSegments->at(surfaceEdgeConnectionsIterator->first)->connections))[surfaceEdgeConnectionsIterator->second[i]] = m_edgeSegments->at(surfaceEdgeConnectionsIterator->second[i]);
		}
	}

	std::cout << "[SegmentsGraph         ] Reading Bin File " << pcsPath << ". ReadSize= [" << (Int64)r_addr_i - file_start << "] " << std::endl;	
	return true;
	//*******************************
	/*Sanity check in order to verify that everything was loaded correctly*/
	//numberOfSurfaceSegments = m_surfaceSegments->size();
	//numberOfEdgeSegments = m_edgeSegments->size();

	//Determine size of file first
	/*int sizeSurfaceSegments = 0;
	for (m_surfaceSegmentsIterator = m_surfaceSegments->begin(); m_surfaceSegmentsIterator != m_surfaceSegments->end(); m_surfaceSegmentsIterator++) 
		sizeSurfaceSegments+=m_surfaceSegmentsIterator->second->sizeInBytes();

	int sizeEdgeSegments = 0;
	for (m_edgeSegmentsIterator = m_edgeSegments->begin(); m_edgeSegmentsIterator != m_edgeSegments->end(); m_edgeSegmentsIterator++) 
		sizeEdgeSegments+=m_edgeSegmentsIterator->second->sizeInBytes();

	int sizeDisjointGraphs = m_disjointSegmentsGraphs->size() * sizeof(int);

	int sizePointsPerSegmentMap = m_pointsPerSegment->size() * 2 * sizeof(int);

	OBB obb;

	int sizeOBBMap = (m_planePrimitivesOBB->size() * sizeof(int)) + (m_planePrimitivesOBB->size() * obb.sizeInBytes());

	size_t file_size = sizeof(int) + sizeSurfaceSegments + 
						sizeof(int) + sizeEdgeSegments + 
						sizeof(int) + sizeDisjointGraphs + 
						sizeof(int) + sizePointsPerSegmentMap +
						sizeof(int) + sizeOBBMap;
	*/	

	/*std::cout << "[SegmentsGraph         ] Reading From Binr File " << _filepath << ". Size= " << r_size << std::endl;
	std::cout << "[SegmentsGraph         ] SurfaceSegmentsSize :: " << sizeSurfaceSegments << " #" << std::endl;
	std::cout << "[SegmentsGraph         ] EdgeSegmentsSize    :: " << sizeEdgeSegments << std::endl;
	std::cout << "[SegmentsGraph         ] DisjointGraphsSize  :: " << sizeDisjointGraphs << std::endl;
	std::cout << "[SegmentsGraph         ] PointsPerSegMapSize :: " << sizePointsPerSegmentMap << std::endl;
	std::cout << "[SegmentsGraph         ] OBBMapSize          :: " << sizeOBBMap << std::endl;*/
}